#ifndef __DEPTH_TO_POINTCLOUD2_H__
#define __DEPTH_TO_POINTCLOUD2_H__

#include "perception/sensors/base/sensor_base.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen; 

class Depth2PointCloud{
    public:
        Depth2PointCloud();
        ~Depth2PointCloud() = default;

        // 设置图像数据
        void setCameraInfo(sensor_msgs::CameraInfo::Ptr& camera_info_ptr);
        // 设置传感器参数数据
        void setParams(const SensorParams& sensor_params);

        // 获取点云数据
        pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud(sensor_msgs::Image::Ptr depth_image_ptr);

        // 是否初始化
        inline bool isInit() const{
            return init_camera_info_ && init_params_;
        }
        // 获取传感器参数
        inline SensorParams getParams(){
            return sensor_params_;
        }
        
    private:
        std::shared_ptr<sensor_msgs::CameraInfo> camera_info_ptr_;
        SensorParams sensor_params_;
        bool init_params_, init_camera_info_;
        Eigen::MatrixXd PoseMatX_, PoseMatY_;
};


#endif